import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose
from rclpy.qos import QoSProfile, ReliabilityPolicy


class OdomCombiner2(Node):

    def __init__(self):
        super().__init__('odom_combiner2')
        # 声明位姿偏移参数（可在启动时通过ROS参数覆盖）
        self.declare_parameters(
            namespace='',
            parameters=[
                ('offset_x', 0.0),
                ('offset_y', 0.0)
            ]
        )
        # 读取位姿偏移量
        self.offset_x = self.get_parameter('offset_x').value
        self.offset_y = self.get_parameter('offset_y').value

        qos_profile = QoSProfile(
            depth=5,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )        
        # 存储最新的位姿和速度数据
        self.latest_pose = None
        self.latest_twist = None
        
        # 订阅两个输入话题
        self.pose_sub = self.create_subscription(
            PoseStamped,
            'ap/pose/filtered',
            self.pose_callback,
            qos_profile)
        
        self.twist_sub = self.create_subscription(
            TwistStamped,
            'ap/twist/filtered',
            self.twist_callback,
            qos_profile)
        
        # 创建输出话题发布器
        self.odom_pub = self.create_publisher(Odometry, 'odom', 10)
        
        # 创建3Hz定时器 (0.333秒周期)
        self.timer = self.create_timer(0.333, self.timer_callback)
        
        self.get_logger().info("Odometry组合器已启动，等待数据...")

    def pose_callback(self, msg):
        """存储校正后的位姿数据"""
        # 创建修正后的位姿副本
        corrected_pose = Pose()
        
        # 应用位置偏移
        corrected_pose.position.x = msg.pose.position.x + self.offset_x
        corrected_pose.position.y = msg.pose.position.y + self.offset_y
        corrected_pose.position.z = 0.0
        
        # 保持原始方向不变
        corrected_pose.orientation = msg.pose.orientation
        
        self.latest_pose = corrected_pose

    def twist_callback(self, msg):
        """存储最新速度数据"""
        self.latest_twist = msg.twist
        # self.get_logger().info('收到速度数据', throttle_duration_sec=1)

    def timer_callback(self):
        """以3Hz频率发布组合后的Odometry数据"""
        if self.latest_pose is None or self.latest_twist is None:
            self.get_logger().debug('等待位姿和速度数据...', throttle_duration_sec=5)
            return
        
        # 创建Odometry消息
        odom_msg = Odometry()
        
        # 设置header
        odom_msg.header.stamp = self.get_clock().now().to_msg()
        odom_msg.header.frame_id = 'odom'
        
        # 设置child_frame_id
        odom_msg.child_frame_id = 'base_footprint'
        
        # 填充位姿数据
        odom_msg.pose.pose = self.latest_pose
        
        # 填充速度数据
        odom_msg.twist.twist = self.latest_twist
        
        # 发布消息
        self.odom_pub.publish(odom_msg)
        self.get_logger().info('发布odom消息 (3Hz)', throttle_duration_sec=5)

def main(args=None):
    rclpy.init(args=args)
    node = OdomCombiner2()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()